/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * @file           : main.c
 * @brief          : Main program body
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2025 STMicroelectronics.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"

#include "dma.h"
#include "gpio.h"
#include "tim.h"
#include "usart.h"


/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "ble_remote.h"
#include "motor.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define ChassisSpeed 120
#define TrackSpeed 60
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
Motor motor[6];    // 电机结构体数组
uint8_t data[11];  // 遥控器数据数组
int16_t speed[6];  // 电机速度数组
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
  if (huart->Instance == USART2) {
    uart_to_remote(data);
  }
}
/* USER CODE END 0 */

/**
 * @brief  The application entry point.
 * @retval int
 */
int main(void) {
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick.
   */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_TIM1_Init();
  MX_TIM2_Init();
  MX_TIM3_Init();
  MX_USART2_UART_Init();
  /* USER CODE BEGIN 2 */
  // 初始化六个520电机
  Motor_Init(&motor[0], &htim1, TIM_CHANNEL_2, EIN1_GPIO_Port, EIN1_Pin,
             EIN2_GPIO_Port, EIN2_Pin);
  Motor_Init(&motor[1], &htim1, TIM_CHANNEL_4, CIN1_GPIO_Port, CIN1_Pin,
             CIN2_GPIO_Port, CIN2_Pin);
  Motor_Init(&motor[2], &htim1, TIM_CHANNEL_3, DIN1_GPIO_Port, DIN1_Pin,
             DIN2_GPIO_Port, DIN2_Pin);
  Motor_Init(&motor[3], &htim1, TIM_CHANNEL_1, FIN1_GPIO_Port, FIN1_Pin,
             FIN2_GPIO_Port, FIN2_Pin);
  Motor_Init(&motor[4], &htim3, TIM_CHANNEL_3, BIN1_GPIO_Port, BIN1_Pin,
             BIN2_GPIO_Port, BIN2_Pin);
  Motor_Init(&motor[5], &htim3, TIM_CHANNEL_4, AIN1_GPIO_Port, AIN1_Pin,
             AIN2_GPIO_Port, AIN2_Pin);
  // 打开pwm输出通道，用来输出pwm波控制舵机
  HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3);
  HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4);

  // 通过指针获取遥控器数据
  const remote_t *r = get_remote_control_point();

  // 使能DMA接收
  HAL_UART_Receive_DMA(&huart2, data, 11);
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1) {
    int16_t s1 = 0, s2 = 0;
    if (r->rocker[1].y_position < 100) {
      if (r->rocker[0].x_position > 100) {
        s1 = ChassisSpeed;
      } else if (r->rocker[0].x_position < -100) {
        s1 = -ChassisSpeed;
      } else {
        s1 = 0;
      }

      if (r->rocker[0].y_position > 100) {
        s2 = ChassisSpeed;
      } else if (r->rocker[0].y_position < -100) {
        s2 = -ChassisSpeed;
      } else {
        s2 = 0;
      }
      speed[0] = s1 + s2;
      speed[1] = -s1 + s2;
      speed[2] = s1 + s2;
      speed[3] = -s1 + s2;
      speed[4] = 0;
      speed[5] = 0;
      if (r->rocker[1].x_position > 100) {  // 右旋转
        speed[0] = ChassisSpeed;
        speed[1] = ChassisSpeed;
        speed[2] = -ChassisSpeed;
        speed[3] = -ChassisSpeed;
      } else if (r->rocker[1].x_position < -100) {  // 左旋转
        speed[0] = -ChassisSpeed;
        speed[1] = -ChassisSpeed;
        speed[2] = ChassisSpeed;
        speed[3] = ChassisSpeed;
      }
      __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, 15);
      __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, 15);
    } else if (r->rocker[1].y_position > 100) {
      if (r->rocker[0].y_position > 100) {
        speed[0] = ChassisSpeed;
        speed[3] = ChassisSpeed;
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, 15);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, 15);
      } else {
        speed[0] = 0;
        speed[3] = 0;
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, 25);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, 5);
      }
      speed[1] = ChassisSpeed;
      speed[2] = ChassisSpeed;
      speed[4] = TrackSpeed;
      speed[5] = TrackSpeed;
    }
    for (int i = 0; i < 6; i++) {
      motor[i].Driver(&motor[i], speed[i]);
    }
    HAL_Delay(100);

    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
 * @brief System Clock Configuration
 * @retval None
 */
void SystemClock_Config(void) {
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
   * in the RCC_OscInitTypeDef structure.
   */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
   */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK |
                                RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
 * @brief  Period elapsed callback in non blocking mode
 * @note   This function is called  when TIM7 interrupt took place, inside
 * HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment
 * a global variable "uwTick" used as application time base.
 * @param  htim : TIM handle
 * @retval None
 */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
  /* USER CODE BEGIN Callback 0 */

  /* USER CODE END Callback 0 */
  if (htim->Instance == TIM7) {
    HAL_IncTick();
  }
  /* USER CODE BEGIN Callback 1 */

  /* USER CODE END Callback 1 */
}

/**
 * @brief  This function is executed in case of error occurrence.
 * @retval None
 */
void Error_Handler(void) {
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1) {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef USE_FULL_ASSERT
/**
 * @brief  Reports the name of the source file and the source line number
 *         where the assert_param error has occurred.
 * @param  file: pointer to the source file name
 * @param  line: assert_param error line source number
 * @retval None
 */
void assert_failed(uint8_t *file, uint32_t line) {
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line
     number, ex: printf("Wrong parameters value: file %s on line %d\r\n", file,
     line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
